#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32.hpp"

float d=0.07;
float PI=3.1415926536;
float vL=0.0,vR=0.0;
class EspRec:public rclcpp::Node
{
private:
    // 声明一个订阅者L
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_L;
    // 声明一个订阅者R
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_R;
    // 声明话题发布者L
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publisher_;
    // 声明定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 收到话题数据的回调函数L
    void vL_callback(const std_msgs::msg::Float32::SharedPtr msg)
    {
        float rpmL=msg->data;
        vL=rpmL*2*PI/60.0*(d/2.0);
    }
    // 收到话题数据的回调函数R
    void vR_callback(const std_msgs::msg::Float32::SharedPtr msg)
    {
        float rpmR=msg->data;
        vR=rpmR*2*PI/60.0*(d/2.0);
    }
    //
    void timer_callback()
    {
        // 创建消息
        std_msgs::msg::Float32MultiArray vLR;
        vLR.data.resize(2);
        vLR.data[0]=vL;
        vLR.data[1]=vR;
        RCLCPP_INFO(this->get_logger(),"encode left speed=%f, encode right speed=%f",vLR.data[0],vLR.data[1]);
        command_publisher_->publish(vLR);
    }
public:
    EspRec(std::string name):Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 创建一个订阅者L订阅话题
        command_subscribe_L = this->create_subscription<std_msgs::msg::Float32>("esp_pro_l", 10, std::bind(&EspRec::vL_callback, this, std::placeholders::_1));
        // 创建一个订阅者R订阅话题
        command_subscribe_R = this->create_subscription<std_msgs::msg::Float32>("esp_pro_r", 10, std::bind(&EspRec::vR_callback, this, std::placeholders::_1));
        // 创建发布者
        command_publisher_=this->create_publisher<std_msgs::msg::Float32MultiArray>("espRE", 10);
        // 创建定时器，500ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&EspRec::timer_callback, this));
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<EspRec>("esp_rec");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}